
#include "classSpotConfig.h"
#include "hardwareIO.h" // this is to have access to mirror limits among other things

SpotConfiguration::SpotConfiguration () {
  stopAllSending();
  numSpots=NBSPOTS;
}

void SpotConfiguration::allStandBy() {
  for(int i=0; i<NBSPOTS; i++){
    spotArray[i].behaviourMode = STAND_BY; // this mode is not very useful: what's the differnce with putting the spot "dead"???
  }
}

void SpotConfiguration::measureMode() {
  spotArray[0].behaviourMode=MEASURE_MODE;
  for(int i=1; i<NBSPOTS; i++){
    spotArray[i].alive = false;
  }
}

/*  perhaps better not to do this, but make a method on hardare.cpp
void SpotConfiguration::scanAreaMode() {
  spotArray[0].behaviourMode=SCAN_AREA; // note: it is up to the pc to stop the scanning area mode. 
  
  spotArray[0].sendOSC=true;
  spotArray[0].sendPos=true;
  spotArray[0].sendingColor=true;
    
  for(int i=1; i<NBSPOTS; i++){
    spotArray[i].alive = false;
  }
}
*/

void SpotConfiguration::stopAllSending() {
  // Default global sending mode for the configuration of spots:
  sendAllOSC=false;
  sendAllSerial=false;
  sendAllMIDI=false;
  
  // DEFAUTL DATA TO SEND for the global configuration - when hardware sending active:
  sendingAllPosition=false;
  sendingAllColor=false; // in fact, mean intensity of the saccade
  sendingAllCollision=false;
  sendingAllTouchWall=false;
  sendingAllAngle=false; 
  
  // Also, STOP PER-SPOT SENDING:
   for(int i=0; i<NBSPOTS; i++){
     spotArray[i].stopAllSending(); // per-spot stop sending
   }
  
}

void SpotConfiguration::allSpotsInit(){
  for(int i=0; i<NBSPOTS; i++){
    //Init parameters: (int threshold, int posX, int posY, int saccadeRadius, int numSample, int id)
    spotArray[i].Init(-1, random(MAX_AD_MIRRORS), random(MAX_AD_MIRRORS), 18 , 20, i); // between 18 and 23 points per saccade is good 
    spotArray[i].vx = random(10,15); 
    spotArray[i].vy = random(10,15);
    spotArray[i].behaviourMode =SIMPLE_BOUNCING;//COUNTOUR_FOLLOWING;
    spotArray[i].bounce_movingDamping=1.0;
    spotArray[i].speedContourFollowing = 0.7; // controls the speed of contour following (proportion of radius)
    spotArray[i].alive = false;
    spotArray[i].showGreenFlash=false;
     spotArray[i].setAutoThreshold(true); // when false, the threshold is fixed (not auto) and can be changed with potA.  
    
    spotArray[i].setPhoenixSpeedMode(true); 
    spotArray[i].setPhoenixSpeed(15,6); //(float _phoenixNormSpeed, float _resetSpeedThreshold) {
  }  
  
  // The following is normally set in the constructor for SpotConfiguration (including, but here we could tweak the default values):
    stopAllSending(); // this sets to false global as well as per-spot sending modes
  
    IO.setGreenPower(LOW);//stop green laser
      
  // FOR TESTS: let's make one spot alive:
  spotArray[0].alive=true; 
      
  // start of THIS config:
  resetTimer();    
      
}

void SpotConfiguration::contourFollowingOnly(int nbSpots){
  for(int i=0; i<nbSpots; i++){
    //Init parameters: (int threshold, int posX, int posY, int saccadeRadius, int numSample, int id)
    spotArray[i].Init(-1, 2000, 2000, 18 , 21, i); // 23 points per saccade is good
    spotArray[i].vx = 15; 
    spotArray[i].vy = 15;
    spotArray[i].behaviourMode = COUNTOUR_FOLLOWING;
    spotArray[i].speedContourFollowing = 0.7; // controls the speed of contour following (proportion of radius)
    spotArray[i].alive = true;
  }  
  for(int i=nbSpots; i<NBSPOTS; i++){
    spotArray[i].alive = false;
  }
}

void SpotConfiguration::snellLawOnly(int nbSpots){
  for(int i=0; i<nbSpots; i++){
    //Init parameters: (int threshold, int posX, int posY, int saccadeRadius, int numSample, int id)
    spotArray[i].Init(-1, 1000, 1000, 30, 25, i);
    spotArray[i].error_angle = -0.25;
    spotArray[i].alive = true;
    spotArray[i].behaviourMode = SNELL_LAW;
    
    // behaviour at the borders:
    spotArray[i].borderMode =  RESET_POSITION;// RETRO_REFLEXION // NORMAL_REFLEXION;
    spotArray[i].placeX=550;  spotArray[i].placeY=CENTER_AD_MIRROR_Y; // special position where the spot appear after it touches the wall (when in RETRO_REFLEXION mode)
    spotArray[i].borderDamping=false;
    spotArray[i].showLimits=false;
     spotArray[i].showGreenFlash=false;
     
    spotArray[i].firstTimeStartRefraction=true; 
    spotArray[i].vx = 9;//10+8*i; 
    spotArray[i].vy = 0;//-8*i;
    spotArray[i].initVx = 9;//10+8*i; 
    spotArray[i].initVy = 0;//-8*i;

    //int xMax, yMax, xMin, yMin; //borde 
    
  }  
  for(int i=nbSpots; i<NBSPOTS; i++){
    spotArray[i].alive = false;
  }
}

void SpotConfiguration::newtonianBouncingOnly(int nbSpots){
  for(int i=0; i<nbSpots; i++){
      spotArray[i].behaviourMode = NEWTON_DYNAMICS;
    
    //Init parameters: (int threshold, int posX, int posY, int saccadeRadius, int numSample, int id)
    spotArray[i].Init(-1, 2100+i*500, 1500+i*500, 30+8*i , 20, i); 
    
    // UpdateSpeed must be used when setting speed in NETWON_DYNAMICS mode, right after updatePosition (or Init of course).  
    spotArray[i].updatePosition(random(1000)+1000, random(1000)+1000); 
    spotArray[i].updateSpeed(-15,10); 
    
     spotArray[i].borderMode=NORMAL_REFLEXION;
    
    spotArray[i].bouncingForceMode=false; // default true
    spotArray[i].bouncingSpecularMode=false;
    spotArray[i].centralForceMode=true; // default mode is CENTRAL force active
    spotArray[i].parallelForceMode=false; // default mode is parallel force inactive
    spotArray[i].particleInteractionMode=false;// default mode 
    spotArray[i].visForceMode=true; // default mode false

    spotArray[i].alive = true;
    spotArray[i].mass=0.3;//+i*0.2;

  }  
  
  // Massive spot: 
   spotArray[0].updatePosition(2000, 2000); 
    spotArray[0].updateSpeed(-20, 10); 
   spotArray[0].mass=0.5;
    spotArray[0].charge=1; // it will attract harder if charge>1 (the others have charge=1)
  
  // kill all the other spots if they were alive: 
  for(int i=nbSpots; i<NBSPOTS; i++){
    spotArray[i].alive = false;
  }
}

void SpotConfiguration::simpleBouncingOnly(int nbSpots){
  for(int i=0; i<nbSpots; i++){
    //Init parameters: (int threshold, int posX, int posY, int saccadeRadius, int numSample, int id)
    spotArray[i].Init(-1, 2000,2000, 18, 20, i); 
    spotArray[i].behaviourMode = SIMPLE_BOUNCING;
    spotArray[i].vx = 18; 
    spotArray[i].vy = 18;
    spotArray[i].alive = true;
  }  
  for(int i=nbSpots; i<NBSPOTS; i++){
    spotArray[i].alive = false;
  }
}

void SpotConfiguration::fastBouncingOnly(int nbSpots){
  for(int i=0; i<nbSpots; i++){
    //Init parameters: (int threshold, int posX, int posY, int saccadeRadius, int numSample, int id)
    spotArray[i].Init(-1, 1000, 1000, 30+8*i , 20, i); 
    spotArray[i].behaviourMode = FAST_BOUNCING;
    spotArray[i].vx = 10; 
    spotArray[i].vy = 3;
    spotArray[i].alive = true;
  }  
  for(int i=nbSpots; i<NBSPOTS; i++){
    spotArray[i].alive = false;
  }
}


// THE CONFIGURATION UPDATE (include updating dynamics of all the spots, read new potentiometers value, and doing other kind of updates - radius, etc). 
void SpotConfiguration::update(){
  
  IO.readPotA(); // this may or may not be used by the spots. Perhaps it would be better to call it only when a button is pressed? (interruption)
  
  for(int i=0; i<NBSPOTS; i++){
    if(spotArray[i].alive){
      
      // Send data (over serial and/or ethernet and/or midi (Serial1) depending on the sending mode:
      spotArray[i].sendData();
    
      // update fixed threshold?
       //if(spotArray[i].behaviourMode == SNELL_LAW){
       //continuous polling of threshold from potentiomenter:
       spotArray[i].updateFixedThreshold((int)255*IO.valuePotA); // the value of readPotA(), also contained in variable valuePotA, is normalized
      // }
      
      // update dynamics:
      spotArray[i].update(spotArray, NBSPOTS);
      
     // change radius?
    if(spotArray[i].behaviourMode == SIMPLE_BOUNCING){ 
        byte radius = 5*sin(200 * timeRunning() / 15000.0+3.1415*i)+18;
        spotArray[i].setRadiusSaccade( radius );
      }
      
/*
      if(spotArray[i].behaviourMode == COUNTOUR_FOLLOWING){ 
        midiPitchAngle(i, spotArray[i].onContour, spotArray[i].target_angle);
        //midiPitchBendAngle(i, spotArray[i].onContour, spotArray[i].target_angle);

        // change size?:
       // if (i==1) {
       // byte radius = 5*sin(200 * (millis()-schedulerTime) / 10000.0)+35;;//15*sin(200 * (millis()-schedulerTime) / 10000.0)+45;
       // spotArray[1].setRadiusSaccade( radius );
       //  if (radius>55) midiCollisionPercussion(i);//midiCollision(i, 0);//
       // }
      }
*/
         
    }    
  }
}



void SpotConfiguration::resetTimer() {
   configAge = millis();
 }
 
unsigned long SpotConfiguration::timeRunning() {
   return(millis()-configAge);
 }


void SpotConfiguration::sendConfData(){
  // (a) Send "packaged" data for all the spots if required: 
  // NOTE: the OSCClass does not allow for OSC-strings, only float and int32 (long); moreover, a maximum of TWO arguments can be passed!!
  //       I will attempt at extending it, but for the time being, lets' just use a serie of longs (we only have a max of four spots!)
  // Ideally, we would send an OSC-string, two bytes per coordinate, so a total (in the worst case) of 4x2=8 bytes: 
  //    /pos ,s XXXX XXXX XXXX XXXX
  // In the meanwhile, the "packaged" data just means the following: 
  // - In the case of sendAllOSC for instance, we would send: 
  //    /pos ,iiiiiiii x1 y1 x2 y2 x3 y3 x4 y4 (assuming all the spots are alive, otherwise there are less arguments). Note that "i" means an unsigned long, i.e. precisely two bytes. There is no much difference with 
  // the string! 
  // But the problem is that the OSC library accept a maximum of TWO arguments; so we need to do something like:  
  //  /1 ii x1 y1 /2 ii x2 y2 ... (which is almost as consuming as sending the data PER SPOT!!! The only different is that we don't send "/pos" each time). 
  // (there is no sendPositionMode or sendAngleMode for the whole config yet, but there will be; it it just that for now we will tweak this by hand. Let's start with the position). 
  if (sendAllOSC) {
    sendMes.setTopAddress("all"); // this indicate that the data is for all the spots (not really necessary)
    for(int i=0; i<NBSPOTS; i++){
      if (sendingAllPosition) {
        if(spotArray[i].alive) {  
           sendMes.setSubAddress(spotArray[i].spotName);
           long  x, y;    //ATTENTION: parameters to setArgs should be long or unsigned long only (not int!!)
           x=(long)spotArray[i].getX(); y=(long)spotArray[i].getY();
           sendMes.setArgs( "ii", &x, &y);
           osc.sendOsc( &sendMes );
          }
      }
      if (sendingAllColor) {
        if(spotArray[i].alive) {  
           sendMes.setSubAddress(spotArray[i].spotName);
           long  maxIntensity=(long)spotArray[i].maxI;
           sendMes.setArgs( "i", &maxIntensity);
           osc.sendOsc( &sendMes );
        }
      }
      if (sendingAllCollision) {
        if(spotArray[i].alive) {  
           sendMes.setSubAddress(spotArray[i].spotName);
           long  coll=(long)(spotArray[i].collision? 1 : 0);
           sendMes.setArgs( "i", &coll);
           osc.sendOsc( &sendMes );
        }
      }
      if (sendingAllTouchWall) {
        if(spotArray[i].alive) {  
           sendMes.setSubAddress(spotArray[i].spotName);
           long  wall=((long)spotArray[i].touchWall? 1 : 0);
           sendMes.setArgs( "i", &wall);
           osc.sendOsc( &sendMes );
        }
      }
       if (sendingAllAngle) {
        if(spotArray[i].alive) {  
           sendMes.setSubAddress(spotArray[i].spotName);
           long  ang=(long)(2048.0*spotArray[i].target_angle/PI); // value from 0 to 1024
           sendMes.setArgs( "i", &ang);
           osc.sendOsc( &sendMes );
        }
      }
      
    }
  }
  
  if (sendAllSerial) {
  }
  
  if (sendAllMIDI) {
  }
  
 // (b) Per-spot sending of data (note: both are NOT exclusive; so if we want just packaged data, we need to make all the spot STOP sending data. 
  for(int i=0; i<NBSPOTS; i++) {
    if(spotArray[i].alive) spotArray[i].sendData();
  }
    
}

 // for tests on scanning area:
void SpotConfiguration::drawScanningArea() {
          IO.writeOutR(0);
          spotArray[0].drawMaxArea(1);
          digitalWrite(greenLaserPin, LOW);
          spotArray[0].drawMaxArea(1);
          digitalWrite(greenLaserPin, HIGH);
          IO.writeOutR(4000);
}

// *** SIMPLE TRANSITION STEP: "Turn off sounds" *** //
void SpotConfiguration::simpleTransitionStep(){
  //turn off all sounds
  myMidi.midiAllNoteOff();
}


// *** TRANSITION STEP: "Concentric Circles" *** //
void SpotConfiguration::transitionStep(){
  
//turn off all sounds
myMidi.midiAllNoteOff();
  
  //draw max area and a saccade 
  IO.writeOutR(0); // turn off the red laser
  
  /*
  // FreeBass CC PRM init:
  // CutOff Max:
  midiControlChange(1, 107, 127);
  // Rezo Null:
  midiControlChange(1, 106, 0);
  // Envelop Modulation null:
  midiControlChange(1, 104, 0);
  // Decay max:
  midiControlChange(1, 102, 127);
  // Disto nulle:
  midiControlChange(1, 105 , 0);
  */
      
  spotArray[0].setNumSamples(40);
  spotArray[0].updatePosition(2000, 2000);
  for(int l=0; l<2; l++){
    for(int i = 1500; i>=0; i-=100){
      spotArray[0].drawMaxArea(1);
      digitalWrite(greenLaserPin, LOW);
      spotArray[0].drawMaxArea(1);
      digitalWrite(greenLaserPin, HIGH);
      spotArray[0].updatePosition(2000, 2000);
      spotArray[0].computeNormalVector(2000, 2000, i);
      digitalWrite(greenLaserPin, LOW);
      spotArray[0].computeNormalVector(2000, 2000, i);
      digitalWrite(greenLaserPin, HIGH);
   
     /*
     // Modulation CutOff & Rezo:
      byte cutoff = (byte)(127- (127.0*i/1500.0));
      midiControlChange(1, 107, cutoff);
      byte rezo = (byte)(127- 127.0*i/1500.0);
      midiControlChange(1, 106, rezo);
      
      byte note = (byte)(48 - (24* i/1600));
      
      midiNoteOff(1, 30);
      midiNoteOn(1, 30, 100);
      
      */
      myMidi.midiNoteOff(10, 35);
      myMidi.midiNoteOn(10, 35, 127);
    
      }
    
    for(int i = 0; i<1500; i+=100){
      spotArray[0].drawMaxArea(1);
      digitalWrite(greenLaserPin, LOW);
      spotArray[0].drawMaxArea(1);
      digitalWrite(greenLaserPin, HIGH);
      spotArray[0].updatePosition(2000, 2000);
      spotArray[0].computeNormalVector(2000, 2000, i);
      digitalWrite(greenLaserPin, LOW);
      spotArray[0].computeNormalVector(2000, 2000, i);
      digitalWrite(greenLaserPin, HIGH);
      
      /*
      byte cutoff = (byte)(127- (127.0*i/1500.0));
      midiControlChange(1, 107, cutoff);
      byte rezo = (byte)(127 - 127.0*i/1500.0);
      midiControlChange(1, 106, rezo);
      
      byte note = (byte)(48- (24* i/1600));
     
      midiNoteOff(1, 30);
      midiNoteOn(1, 30, 100);
     */
      myMidi.midiNoteOff(10, 35);
      myMidi.midiNoteOn(10, 35, 127);
  
    }
    
    
  // Circle bounce on wall >> 1 note on FB33
  //midiNoteOff(0, 30);
  //midiNoteOn(0, 30, 127);
  }
  
  spotArray[0].setNumSamples(18);  
  IO.writeOutR(4000);
}

void SpotConfiguration::killAll() {
 for(int i=0; i<NBSPOTS; i++) spotArray[i].alive = false;
}


// ===============================================  SPECIAL TAILORED DEMO MODES ================================================================
// =============================================================================================================================================

void SpotConfiguration::oneContourFollowing() {
      
      contourFollowingOnly(1);
      spotArray[0].speedContourFollowing = 0.7; // controls the speed of contour following (proportion of radius)
      spotArray[0].showLimits=false; // avoid having the green contour when touching the "walls" (because we will project from the side). 
      spotArray[0].showGreenFlash=false;
      IO.setGreenPower(LOW); // stop green laser
      
      // AFTER setting the spot, set the threshold value and mode:
      spotArray[0].setAutoThreshold(true);
      //Use potentiometer threshold:
      // spotArray[0].setAutoThreshold(false);
      // spotArray[0].updateFixedThreshold(potFixThreshold);
      // ... or fixed threshold:
      //spotArray[0].updateFixedThreshold(FIXED_THRESHOLD);
      
      // Change midi mode:
      myMidi.midiProgramChange (4, 1);
     
}

void SpotConfiguration::twoContourFollowing() {
    
      contourFollowingOnly(2);
      spotArray[0].showLimits=false; spotArray[1].showLimits=false;
        spotArray[0].showGreenFlash=false;spotArray[1].showGreenFlash=false;
      // make one spot turn in the opposite direction:
      spotArray[1].speedContourFollowing = -0.6; // controls the speed of contour following (proportion of radius)
      digitalWrite(greenLaserPin, HIGH); // laser off
      
       spotArray[1].updatePosition(3000, 3000);
      
      // AFTER setting the spot, set the threshold value and mode:
       spotArray[0].setAutoThreshold(true);
       spotArray[1].setAutoThreshold(true);
      
      myMidi.midiProgramChange (4, 4);
}

void SpotConfiguration::oneSimpleBouncing() {
       simpleBouncingOnly(1);
       spotArray[0].showLimits=false;
       spotArray[0].setPhoenixSpeed(16,5);  // New for seikai ichi: start speed is 0 and working speed will be loaded with firstTimeSpeedNorm (the fist input parameter) when collision==true for the first time. This mode
       // is reset when speed norm becomes smaller than the second parameter (this is to take into account damping that will make the particle stop again)
       
       spotArray[0].vx=8;
       spotArray[0].vy=7;
       spotArray[0].bounce_collisionDamping=1.00; // 1 means no damping when collision (when in bouncing mode). Larger than one, this means it will accelerate. Note: max speed is the initial "phoenix" speed!
       spotArray[0].bounce_movingDamping=0.9995; // 1 means no damping as it moves (when in bouncing mode). 
       spotArray[0].showGreenFlash=false; // false: faster
       spotArray[0].updatePosition(2000, 1500);
       digitalWrite(greenLaserPin, LOW); // laser activated
       
       // AFTER setting the spot, set the threshold value and mode:
        spotArray[0].setAutoThreshold(true);
}

void SpotConfiguration::twoSimpleBouncing() {
       simpleBouncingOnly(2);
       spotArray[0].showLimits=false;
       spotArray[0].setPhoenixSpeed(16,5);  // New for seikai ichi: start speed is 0 and working speed will be loaded with firstTimeSpeedNorm (the fist input parameter) when collision==true for the first time. This mode
       // is reset when speed norm becomes smaller than the second parameter (this is to take into account damping that will make the particle stop again)
       spotArray[0].vx=8; spotArray[0].vy=7;
       spotArray[0].bounce_collisionDamping=1.00; // 1 means no damping when collision (when in bouncing mode). Larger than one, this means it will accelerate. Note: max speed is the initial "phoenix" speed!
       spotArray[0].bounce_movingDamping=0.9995; // 1 means no damping as it moves (when in bouncing mode). 
       spotArray[0].showGreenFlash=false; // false: faster
       spotArray[0].updatePosition(2000, 1500);
    
       
       spotArray[1].showLimits=false;
       spotArray[1].setPhoenixSpeed(16,5);  // New for seikai ichi: start speed is 0 and working speed will be loaded with firstTimeSpeedNorm (the fist input parameter) when collision==true for the first time. This mode
       // is reset when speed norm becomes smaller than the second parameter (this is to take into account damping that will make the particle stop again)
       spotArray[1].vx=-8; spotArray[1].vy=-7;
       spotArray[1].bounce_collisionDamping=1.00; // 1 means no damping when collision (when in bouncing mode). Larger than one, this means it will accelerate. Note: max speed is the initial "phoenix" speed!
       spotArray[1].bounce_movingDamping=0.9995; // 1 means no damping as it moves (when in bouncing mode). 
       spotArray[1].showGreenFlash=false; // false: faster
       spotArray[1].updatePosition(3000, 3000);
       
      // AFTER setting the spot, set the threshold value and mode:
       spotArray[0].setAutoThreshold(true);
      spotArray[1].setAutoThreshold(true);
       
       digitalWrite(greenLaserPin, HIGH); // laser off
}

void SpotConfiguration::oneSnellLaw() {
       snellLawOnly(1);
       
       // AFTER setting the spot, set the threshold value and mode:
      //Use potentiometer threshold:
      spotArray[0].setAutoThreshold(false);
      spotArray[0].updateFixedThreshold(255*IO.valuePotA); // in fact, it will be continuously updated in the loop
      // ... or fixed threshold:
      //spotArray[0].updateFixedThreshold(FIXED_THRESHOLD);
}
       
